#!/usr/bin/env python
# -*- coding: UTF-8 -*-
'''
@Project ：刘骏轩2025讯飞 
@File ：LineControl.py
@Author ：ljx
@Date ：2025/7/25 下午9:30 
'''

from GetCvPicture import PicGetter
import cv2
import rospy
from geometry_msgs.msg import Twist
from LineFind import LineFinder
import threading
import numpy as np
from std_srvs.srv import Trigger, TriggerResponse
import time
from sensor_msgs.msg import LaserScan
import math



class PD_Controler:
    def __init__(self):
        self.KP = 0.85
        self.KD = 0.45
        self.last_error = 0
        self.last_last_error = 0
        self.max_output = 2.0
        self.target = 0
        self.output = 0

    def Revise(self, now):
        """
        PD 控制角速度
        :param now:当前值，now = 中点坐标的平均值减去320  now > 0 :则说明中线在小车的靠右位置 小车需右转 , now < 0 : 中线在小车的靠左位置 , 小车需左转
        :return: 控制输出
        """
        # 采用增量式PD
        # 角速度为负时,是顺时针，即向右偏 ; 为正时,是逆时针，即向左偏
        error = self.target - now
        self.output += self.KP * (error - self.last_error) + self.KD * (error + self.last_last_error - 2 * self.last_error)

        self.output = np.clip(self.output, -self.max_output, self.max_output)
        self.last_last_error = self.last_error
        self.last_error = error

        return self.output


class State:
    RIGHT = 1
    FORWARD_AROUND = 2
    LEFT = 3
    OVER = 4


class Lidar():
    def __init__(self, pub_cmd):
        self.pub_cmd = pub_cmd
        self.subscriber = rospy.Subscriber('/scan', LaserScan, self.lidar_callback)
        self.last_msg = None
        self.avoidance_completed = False  #记录避障是否完成
        self.state = State.RIGHT
        self.count = 0
        self.distance = 0.36  # 安全距离阈值
        self.avoidance_active = False

    def lidar_callback(self, msg):
        self.last_msg = msg

    def start_avoidance(self):
        """开始避障过程"""
        if not self.avoidance_active and not self.avoidance_completed:
            rospy.loginfo("Starting obstacle avoidance")
            self.avoidance_active = True
            self.state = State.RIGHT
            self.count = 0
            return True
        return False

    def obstacle_detected(self):
        # 如果已经完成避障，直接返回False
        if self.avoidance_completed or self.avoidance_active:
            return False

        if self.last_msg is None:
            return False

        center = len(self.last_msg.ranges) // 2
        detected  = False

        start_angle = center - 10
        end_angle = center + 10

        for i in range(start_angle, end_angle):
            if 0 <= i < len(self.last_msg.ranges):
                distance = self.last_msg.ranges[i]
                # 检查有效距离和障碍物
                if 0 < distance < self.distance and not math.isinf(distance):
                    detected  = True
                    break

        return detected

    def execute_avoidance(self):
        if not self.avoidance_active or self.avoidance_completed:
            return False

        twist = Twist()
        twist.linear.x = twist.linear.y = twist.angular.z = 0.0

        if self.state == State.RIGHT:
            twist.linear.y = 0.38  # 向右平移
            self.count += 1
            if self.count >= 20:
                rospy.loginfo("Transition to FORWARD_AROUND")
                self.state = State.FORWARD_AROUND
                self.count = 0

        elif self.state == State.FORWARD_AROUND:
            twist.linear.x = 0.38  # 向前移动
            self.count += 1
            if self.count >= 28:
                rospy.loginfo("Transition to LEFT")
                self.state = State.LEFT
                self.count = 0

        elif self.state == State.LEFT:
            twist.linear.y = -0.40  # 向左平移
            self.count += 1
            if self.count >= 22:
                rospy.loginfo("Transition to OVER - Avoidance completed")
                self.state = State.OVER
                self.avoidance_completed = True
                self.avoidance_active = False  # 设置避障完成标志
                self.count = 0
                return True

        self.pub_cmd.publish(twist)
        return False

class Line_Controler:
    def __init__(self):
        self.cap = PicGetter()
        self.PD_control = PD_Controler()
        self.finder = LineFinder()
        # 表示是否在巡线
        self.active = False
        self.cmd_pub = None
        self.control_thread = None
        # 发布速度消息
        self.cmd_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)

        self.emergency = False
        self.lidar = Lidar(self.cmd_pub)


    def start(self):
        """
        这个服务只会启动一次，所以没有避免考虑过多，比如说self.image_sub 和 self.cmd_pub 重复申请的问题
        :return:
        """
        if self.active:
            rospy.logwarn("Controller is already running")
            return
        self.active = True
        # self.control_thread = threading.Thread(target=self.run)
        # self.control_thread.daemon = True  # 设置为守护线程
        # self.control_thread.start()
        # 开始订阅图片
        self.cap.start()
        rospy.loginfo("Control thread started")
        self.run()

    def run(self):
        # rate = rospy.Rate(20)  # 25Hz

        while not rospy.is_shutdown() and self.active:
            img = self.cap.Get()
            if img is None:
                continue
            try:
                error = self.finder.solve(img,"test")

                if not self.lidar.avoidance_completed and not self.lidar.avoidance_active:
                    if self.lidar.obstacle_detected():
                        self.lidar.start_avoidance()

                    # 3. 如果避障已启动，执行避障动作
                if self.lidar.avoidance_active:
                    completed = self.lidar.execute_avoidance()
                    if completed:
                        rospy.loginfo("Resuming line following")
                        rospy.sleep(0.05)
                else:
                    msg = Twist()
                    if self.finder.area == "transition":
                        msg.linear.x = 0.1
                        # 现在把速度反过来给，为了可以去到直线区域
                        if self.finder.outLine == "right":
                            msg.angular.z = 0.15
                        else:
                            msg.angular.z = -0.15
                    elif self.finder.area == "transition_roll":
                        msg.angular.z = 0.3
                    elif self.finder.area == "end":
                        msg.linear.x = 0.1
                        msg.linear.y = -0.05
                        if not self.emergency:
                            self.emergency = True
                            rospy.loginfo("Attention!!!")
                    elif self.finder.area == "stop":
                        rospy.loginfo("Line Finish")
                        break

                    else:
                        if self.emergency:
                            break
                        msg.linear.x = 0.25
                        if error is not None:
                            output = self.PD_control.Revise(error) if abs(error) > 0.05 else 0
                            rospy.loginfo(f"output:{output}")
                            msg.angular.z = output

                    self.cmd_pub.publish(msg)

            except Exception as e:
                rospy.logerr(f"控制出错: {e}")
                rospy.sleep(0.1)  # 出错时稍作暂停

            # rate.sleep()

        # 停止机器人
        stop_msg = Twist()
        self.cmd_pub.publish(stop_msg)

        self.active = False

        rospy.loginfo("Control thread stopped")


class LineService:
    def __init__(self):
        self.control = Line_Controler()
        self.service = None

    def doRequest(self, req):
        if self.control.active:
            return TriggerResponse(False, "Already running")

        # 运行控制逻辑
        self.control.start()

        return TriggerResponse(True, "Line control started")

    def start(self):
        rospy.loginfo("Line Service is ready")
        self.service = rospy.Service("LinePatrolService", Trigger, self.doRequest)


if __name__ == '__main__':
    rospy.init_node('LinePatrol', anonymous=True)

    service = LineService()

    service.start()

    rospy.spin()
